package com.DataProcess;

import android.util.Log;

import com.pudu.json.Json2PC;
import com.pudu.mydemo.nativeClass.NativeClass;
import com.pudu.pdrobot.App;
import com.sensors.Encoder;
import com.sensors.Gyroscope;
import com.sensors.UWB;

import java.util.Arrays;
import java.util.Timer;
import java.util.TimerTask;

/**
 * Created by tjpu on 2016/10/12.
 */
public class CanDataProcess {
    static String thisLogTag = "CanDataProcess";

    public CanDataProcess(Timer timer) {
        initTimerCheckReceive(timer);
    }
    CanDataDecode canData = new CanDataDecode();
    public void putOrigin(byte[] dataSource) {
        if (Robot.uwbStation == null || Robot.track == null) {
            Log.e(thisLogTag,"no map");
            return;
        }
        byte [] data = new byte[dataSource.length];
        System.arraycopy(dataSource,0,data,0,data.length);
        canData.decode(data);

        Log.i(thisLogTag,"canData decode succeed");
        switch (canData.type) {
            case CanDataType.SPEED:
                speed();
                break;
            case CanDataType.UWB:
                uwb();
                break;
            case CanDataType.IRD:
                ird();
                break;
            case CanDataType.GYRO:
                gyro();
                break;
            default:
        }
        if (App.instance.myService != null)
            App.instance.myService.SendDataToClient(data);
    }

    Encoder encoder = new Encoder();
    void speed() {
        encoder.putOrigin(canData.speed);
        Robot.speed = encoder.getMetric();
        Robot.tangentialVel = (Robot.speed[0] + Robot.speed[1]) / 2.0;  //中心点切向速度
        Robot.axialVel= (Robot.speed[1] - Robot.speed[0]) / Robot.WHEELS_THREAD;    //轴向速度
        Json2PC.sendYaw();

        double distanceL = Robot.speed[0] * Encoder.PERIOD;
        double distanceR = Robot.speed[1] * Encoder.PERIOD;
        if(distanceL < 0.01 && distanceR < 0.01)
            NativeClass.cSetWheelSpeed2(distanceL, distanceR, Robot.gyroAngle[2], 0.0);
        else
            NativeClass.cSetWheelSpeed2(distanceL, distanceR, Robot.gyroAngle[2], 0.0);

        Robot.position = NativeClass.cGetCenter();
        Robot.dir = NativeClass.cGetDir();
        Robot.aimPoint = NativeClass.cAimPoint(Robot.position, Robot.dir);
        if(Robot.isAuto)
            Robot.targetSpeed = NativeClass.cTrack(Robot.position, Robot.dir);
        Json2PC.sendPose();

        Log.i(thisLogTag,"robot pos:"+ Arrays.toString(Robot.position));
        Log.i(thisLogTag,"robot dir:"+Arrays.toString(Robot.dir));
        Log.i(thisLogTag,"robot aim:"+Arrays.toString(Robot.aimPoint));
    }

    UWB uwb = new UWB();
    void uwb() {

        uwb.putOrigin(canData.uwbID1, canData.uwbDistance1);
        if (uwb.isReady) {
           uwbReady();
        }

        uwb.putOrigin(canData.uwbID2, canData.uwbDistance2);
        if (uwb.isReady) {
            uwbReady();
        }
    }
    void uwbReady() {
        double[] distance;
        distance = uwb.getMetric();
        Robot.tagPosition = NativeClass.cLocateTag(distance);
        Json2PC.sendTag();
        NativeClass.cLocate(distance);
        Json2PC.sendUWBS(distance);
        Log.i(thisLogTag, "uwb:"+Arrays.toString(distance));
        uwb.reset();
    }

    void ird() {

    }

    Gyroscope gyroscope = new Gyroscope();
    void gyro() {
        gyroscope.putOrigin(canData.gyroscope);
        Robot.gyroAngleVel = gyroscope.getMetric();
        Robot.gyroAngle = gyroscope.getAngle();
        Log.i(thisLogTag,"gyro: " + Arrays.toString(gyroscope.getMetric()));
    }

    void initTimerCheckReceive(Timer timer) {
        timer.schedule(new TimerTask() {
            @Override
            public void run() {
                encoder.checkReceive();
                uwb.checkReceive();
                gyroscope.checkReceive();
                Log.i(thisLogTag,"timer current thread: "+Thread.currentThread().getId());
            }
        },0,100);
    }
}
